#include "navi.h"



//导航初始化一系列参数
int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos)
{

	if(o ==NULL)  return DEVICE_ERR;
	if(param == NULL) return DEVICE_ERR;
//	while(!Action_ready)
//	{		
//     TIM4->CCR3 = 9999;//等待全场定位初始化完戄1�7
//		osDelay(5);
//	}
	
//    TIM4->CCR3 = 0;
//	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
//	
	o->param = param;
	/*全场定位pid初始化*/
	//全场定位跑路径的xy方向速度环pid
	PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
	PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
	 /* 初始化角度纠正PID */
	PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
	PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
	//底盘xy方向位置环pid初始化
	//内环
	PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
	PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
	//外环
	PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
	PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));


	
	//底盘路径速度pid初始化
	PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
	//全场定位底盘坐标初始化
	o->chassis_pos->pos_x=0;
	o->chassis_pos->pos_y=0;	
	o->chassis_pos->pos_lastX=0;
	o->chassis_pos->pos_lastY=0;

	o->chassis_pos = pos;
	o->chassis_map = o->param->path;
	//设置全场定位允许的误差范围
	o->state.mistake = POS_ALLOW_MISTAKE;
	o->state.moveState = START;
	o->state.points_num = param->path_num;
	//标志位初始化
	o->POS_IS_CPT = NO;

	return DEVICE_OK;
}
//利用C板imu纠正角度
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
{
	fp32 delta_angle,delta_w;
	//外环角度纠正
	delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
	//内环速度纠正
	delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
	return delta_w;
}

//底盘xy速度计算(全场定位)
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
		o->final_out.vx = -(PID_calc(&o->pid_PosSpeed_x, o->vx,vx_set));//根据遥控器的方向选择正负叄1�7
		o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->vy,vy_set);
	    o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
}


//定点控制
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
{
	fp32 delta_x,delta_y;
	fp32 delta_vx,delta_vy;
	fp32 vx_set,vy_set;//x,y,w目标速度
	
	//x
	delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
	delta_vx = PID_calc(&o->pid_pos_x_inner,o->vx,delta_x);
	
	//y
	delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
	delta_vy = PID_calc(&o->pid_pos_y_inner,o->vy,delta_y);
	
	vx_set = delta_vx;
	vy_set = delta_vy;
	
	POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
}

//判断是否到达函数(全场定位)
bool isArrive(point_t p,ops_t *o)
{
	uint16_t xArrive = 0, yArrive = 0;
	xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
	yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
	if(xArrive && yArrive) return true;
	else return false;
}
//寻迹函数
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
{

	if(o == NULL) return DEVICE_ERR;
	if (ops_out == NULL) return DEVICE_ERR;


 static int cnt =0;//计数标志位
 static fp32 chassis_speed_set;//底盘速度设置
 static fp32 SIN,COS;//方向解算
 static fp32 world_vx,world_vy;//x,y方向分解速度
 static fp32 chassis_vx,chassis_vy;//底盘xy分速度
 static fp32 distance;//两点之间的距离差
// static fp32 yaw_angle_set = 0;	
	if(o->POS_IS_CPT == NO)//路径未跑完
	{
		if(o->state.moveState ==START)
		{
			//更新路径状态
			o->state.moveState =MOVING;
			cnt =0;
			
		}
		if(cnt<(o->state.points_num-1))
	{
		//计算距离
		distance = sqrt((o->chassis_map[cnt+1].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt+1].x - o->chassis_pos->pos_x)
										+(o->chassis_map[cnt+1].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt+1].y - o->chassis_pos->pos_y));
		
//		chassis_speed_set = chassis_move.chassis_map->path[cnt].v;//此处设定为固定的期望速度
		  chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
		
		//速度分解
		SIN = (o->chassis_map[cnt+1].y - o->chassis_pos->pos_y)/distance;
		COS = (o->chassis_map[cnt+1].x - o->chassis_pos->pos_x)/distance;
		
		world_vx = chassis_speed_set * COS;
		world_vy = chassis_speed_set * SIN;
		
//		chassis_vx = world_vx;
//		chassis_vy = world_vy;
		//世界坐标转机器坐标系转换
		chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
		chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
		
		//判断是否到达(全场定位)
		if(!isArrive(o->chassis_map[cnt+1],o))
		{ 
			//未到达目的地
		  POS_chassis_set(o,chassis_vx,chassis_vy,o->chassis_map[cnt+1].angle);
		}
		else
		{	
       hold_point(o,o->chassis_map[cnt+1],o->chassis_map[cnt+1].angle);	//调用该函数使临近误差值的死区快��响庄1�7		
			cnt++;
		}
		
   }
		else
	{
			o->POS_IS_CPT =YES;
	}
  }
	else if(o->POS_IS_CPT == YES)//路径完成,定点控制
	{

    hold_point(o,o->chassis_map[o->state.points_num-1],o->chassis_map[o->state.points_num-1].angle);
	}

	ops_out->out.Vx= o->final_out.vx;
	ops_out->out.Vy = o->final_out.vy;
	ops_out->out.Vw = o->final_out.yaw_angle;

	return DEVICE_OK;
}
